#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>

//#define MODE_DEBUG 0
#define MODE_RUN   1

/* 
 * signal data is used when connecting with PC
 * [SIGNAL_HELLO] <--> shake hands with PC
 * [SIGNAL_DATASEND] <--> send data to PC & receive data from PC 
 */
#define SIGNAL_HELLO    0xF1
#define SIGNAL_DATASEND 0xF2

/* the definition of all devices linking pin */
#define _595_1_latchPin 2
#define _595_1_clockPin 3
#define _595_1_dataPin 4
#define servoPin 5
#define tiltor2Pin  6
#define signal6Pin 8
#define gear3Pin A0

//for sigment_8_led
#define RIGHT LSBFIRST
#define LEFT  MSBFIRST


Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(1);
Servo speedPointer;
float xAccel, zAccel, direct;
byte tempData, velocity, accelerate, brake, gear;



/* function: checkGear
 *  usage:check the value of gear
 *  0 means low accelerate
 *  1 means high accelerate
 *  2 means free
 *  3 means back off
 */
void checkGear(void);
/* function: showByLed
 *  usage: control the sigment_8_led device
 *  when the steering wheel turns right, it will light 
 *  lights on the right side; ohterwise, the left 
 *  side. The more wheel rotates, the more leds will 
 *  be turned on.
 */
void showByLed(float);
/* function: showByServo
 *  usage: turn the servo to a specific position,
 *  depending on the value of speed */
void showByServo(byte);
/* function: checkAndSetAccelerator
 * usage: set ADXL345 configure */
void checkAndSetAccelerator(void);
/* functions:adafruit ADXL345 */
void displaySensorDetails(void);
void displayDataRate(void);
void displayRange(void);






void setup(void) 
{
    accelerate = brake = false;
    xAccel = zAccel = 0;
    
    //check whether the serial port is opened successfully
    do 
    {
        Serial.begin(9600);
    } while(!Serial);

    //check whether the ADXL345 accelerator is linked successfully
    checkAndSetAccelerator();

    //set the state of servo
    speedPointer.attach(servoPin);
    //28 for full speed[140] & 178 for initial speed[0]
    speedPointer.write(178);
    
    for (int i = 0; i < 2; i++)
        pinMode(tiltor2Pin+i, INPUT);
    for (int i = 0; i < 3; i++)
        pinMode(gear3Pin+i, INPUT);

    for (int i = 0; i < 6; i++) {
        pinMode(signal6Pin+i, OUTPUT);
        digitalWrite(signal6Pin+i, LOW);
    }
    
    pinMode(_595_1_latchPin, OUTPUT);
    pinMode(_595_1_clockPin, OUTPUT);
    pinMode(_595_1_dataPin, OUTPUT);
    
    digitalWrite(_595_1_latchPin, LOW);
    shiftOut(_595_1_dataPin, _595_1_clockPin, MSBFIRST, 0);
    digitalWrite(_595_1_latchPin, HIGH);
    
    
#ifdef MODE_RUN
    //interact with the computer:
    //wait until the computer sends the starting signal
    //run the followed block to check if the received data is SIGNAL_HELLO
    bool shakeHands = false;
    do{
        if (Serial.available()){
            if ((tempData = Serial.read()) == SIGNAL_HELLO) {
                Serial.write(SIGNAL_HELLO);
                shakeHands = true;
            }
        }
    } while(!shakeHands);
#endif
}

void loop(void) 
{
    do
    {
#ifdef MODE_RUN
        if (!(Serial.available())) break;
        if ((tempData = Serial.read()) != SIGNAL_DATASEND) break;
        //wait untill the next true data be sent
        while (!(Serial.available()));
        velocity = Serial.read();
        //turn the needle on the dashboard
        showByServo(velocity);
#endif
        // Get a new sensor event
        sensors_event_t event;
        accel.getEvent(&event);

        /*
         * in this step, we do not need yAccel;
         * the angle of rotated steering wheel equals to
         *        -xAccel/zAccel
         * in the end, we set the value into range
         * [-70, 70]. If you want to know how to calculate
         * the data, please refer to 
         *      UltraRealisticDriverSimulator.pdf.
         */
        xAccel = event.acceleration.x;
        zAccel = event.acceleration.z;
        direct = -xAccel / zAccel;
        if (zAccel > 0) direct = xAccel<0? -70:70;

        accelerate = !digitalRead(tiltor2Pin);
        brake = !digitalRead(tiltor2Pin+1);
        digitalWrite(signal6Pin, accelerate);
        digitalWrite(signal6Pin+1, brake);

        checkGear();

#ifdef MODE_DEBUG
        Serial.print("accelerate");Serial.println(accelerate);
        Serial.print("brake:");Serial.println(brake);
        Serial.print("gear");Serial.println(gear);
        Serial.println(analogRead(A2));
        Serial.print(xAccel);Serial.print(" ");
        Serial.println(zAccel);
#endif

#ifdef MODE_RUN
        Serial.write(accelerate);
        Serial.write(brake);
        Serial.write(gear);
        Serial.write((byte*)&direct, 4);
#endif

        showByLed(direct);
    } while(false);
}

void showByServo(byte velocity)
{
    int showSpeed = map(velocity, 0, 255, 2, 152);
    speedPointer.write(180 - showSpeed); 
}

void showByLed(float direct)
{
    //do not need to considerate left or right
    //so we take an absolute value
    float absDirect = abs(direct);
    byte showLed = 0;

    /*
     * (0.4, 0.8] for 1 led
     * (0.8, 1.5] for 2 leds 
     * (1.5, 4.0] for 3 leds
     * (4.0, +infinity] for 4 leds
     */
    if (absDirect > 0.4) showLed |= 0x08;
    if (absDirect > 0.8) showLed |= 0x04;
    if (absDirect > 1.5) showLed |= 0x02;
    if (absDirect > 4.0) showLed |= 0x01;

    //xAccel<0 when the steering wheel turns left
    //xAccel>0 when the steering wheel turns right
    digitalWrite(_595_1_latchPin, LOW);
    shiftOut(_595_1_dataPin, _595_1_clockPin, xAccel<0? LEFT:RIGHT, showLed);
    digitalWrite(_595_1_latchPin, HIGH);
}

void checkGear(void) {
    bool flag;
    int i, j;
    for (i = 0; i < 3; i++) {
        flag = false;
        for (j = 0; j < 200; j++) {
            //bigger than 95 indicates that a barrier appears
            //so that means we change the vehicle's gear
            if (analogRead(gear3Pin+i) > 95) {
                flag = true;
                break;
            }
        }
        if (flag) break;
    }
    //light the corresponding led on the brashboard
    for (gear = i, j = 0; j < 4; j++) {
        if (gear == j) digitalWrite(signal6Pin+2+j, HIGH);
        else digitalWrite(signal6Pin+2+j, LOW);
    }
}

void checkAndSetAccelerator(void) {
    if(!accel.begin())
    {
#ifdef MODE_DEBUG
        Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
#endif
        while(1);
    }
    
#ifdef MODE_DEBUG
    // Initialise the sensor
    Serial.println("Accelerometer Test"); Serial.println("");
#endif

    // Set the range to whatever is appropriate for your project
    accel.setRange(ADXL345_RANGE_16_G);
    // accel.setRange(ADXL345_RANGE_8_G);
    // accel.setRange(ADXL345_RANGE_4_G);
    // accel.setRange(ADXL345_RANGE_2_G);

#ifdef MODE_DEBUG
    // Display some basic information on this sensor
    displaySensorDetails();
    
    // Display additional settings (outside the scope of sensor_t)
    displayDataRate();
    displayRange();
    Serial.println("");
#endif
}

void displaySensorDetails(void)
{
    sensor_t sensor;
    accel.getSensor(&sensor);
    Serial.println("------------------------------------");
    Serial.print  ("Sensor:       "); Serial.println(sensor.name);
    Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
    Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
    Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" m/s^2");
    Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" m/s^2");
    Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" m/s^2");  
    Serial.println("------------------------------------");
    Serial.println("");
    delay(500);
}

void displayDataRate(void)
{
    Serial.print("Data Rate:    "); 
    
    switch(accel.getDataRate())
    {
        case ADXL345_DATARATE_3200_HZ:Serial.print  ("3200 "); break;
        case ADXL345_DATARATE_1600_HZ:Serial.print  ("1600 "); break;
        case ADXL345_DATARATE_800_HZ: Serial.print  ("800 ");  break;
        case ADXL345_DATARATE_400_HZ: Serial.print  ("400 ");  break;
        case ADXL345_DATARATE_200_HZ: Serial.print  ("200 ");  break;
        case ADXL345_DATARATE_100_HZ: Serial.print  ("100 ");  break;
        case ADXL345_DATARATE_50_HZ:  Serial.print  ("50 ");   break;
        case ADXL345_DATARATE_25_HZ:  Serial.print  ("25 ");   break;
        case ADXL345_DATARATE_12_5_HZ:Serial.print  ("12.5 "); break;
        case ADXL345_DATARATE_6_25HZ: Serial.print  ("6.25 "); break;
        case ADXL345_DATARATE_3_13_HZ:Serial.print  ("3.13 "); break;
        case ADXL345_DATARATE_1_56_HZ:Serial.print  ("1.56 "); break;
        case ADXL345_DATARATE_0_78_HZ:Serial.print  ("0.78 "); break;
        case ADXL345_DATARATE_0_39_HZ:Serial.print  ("0.39 "); break;
        case ADXL345_DATARATE_0_20_HZ:Serial.print  ("0.20 "); break;
        case ADXL345_DATARATE_0_10_HZ:Serial.print  ("0.10 "); break;
        default:
            Serial.print  ("???? "); break;
    }  
    Serial.println(" Hz");  
}

void displayRange(void)
{
    Serial.print("Range:         +/- "); 
    switch(accel.getRange())
    {
        case ADXL345_RANGE_16_G:Serial.print  ("16 "); break;
        case ADXL345_RANGE_8_G: Serial.print  ("8 ");  break;
        case ADXL345_RANGE_4_G: Serial.print  ("4 ");  break;
        case ADXL345_RANGE_2_G: Serial.print  ("2 ");  break;
        default:
            Serial.print  ("?? "); break;
    }  
    Serial.println(" g");  
}
